module Navigator where
import Language.Atom.Expressions
import Language.Atom
import Data.Word
import Data.Int
import Text.Printf
import Control.Monad
include lib = "#include <" ++ lib ++ ">"
include' lib = "#include \"" ++ lib ++ "\""

fakeHeaders :: String
fakeHeaders = unlines
  [ include' "fakearduino.h"
  , "int newestRight = 0;"
  , "int newestLeft = 0;"
  , "int compass = 850;"
  ]

headerFiles :: String
headerFiles = unlines
  [ "// generated with Atom"
  , include "WProgram.h"
  , include' "Compass.h"
  , "#define COMPASS compass.heading()"
  , "#define STEERING steeringServo.write"
  , "#define THROTTLE throttleServo.write"
  , "#define assert //"
  , "Compass compass;"
  ]

header :: String -> String
header x = unlines   
  [ x
  , "void loop(void);"
  , "int targetHeading = 1800;"
  , "int actualHeading = 0;"  
  , "int steering = 0;"
  , "int bearing = 0;"
--  , "void main() __attribute__ ((noreturn));"
  ]

driver :: String
driver = unlines
  [ "void main() {"
  , "  init();"
  , "  compass.initialize();"
  , "  for (;;) {"
  , "    loop();"
  , "  }"
  , "}"
  ]

prePostCode :: [Name] -> [Name] -> [(Name, Type)] -> (String, String)
prePostCode _ _ _ = (header headerFiles, driver)


-- Override some default values
config :: Config
config = defaults {
    cCode = prePostCode,
    cFuncName = "loop",
    cRuleCoverage = False, 

   -- The (u)intXX_t types are defined in
    -- stdint.h--which we include from blink.h
    cType     = \t -> case t of
                       Bool   -> "uint8_t"
                       Int8   -> "int8_t"
                       Int16  -> "int16_t"
                       Int32  -> "int32_t"
                       Int64  -> "int64_t"
                       Word8  -> "uint8_t"
                       Word16 -> "uint16_t"
                       Word32 -> "uint32_t"
                       Word64 -> "uint32_t" -- heh heh
                       Float  -> "float"
                       Double -> "double"
}


t1 = defaultTest {
          testbench = arraySumming
         ,declCode = header fakeHeaders
        }

t2 = defaultTest {
          testbench = steeringVsBearing
         ,declCode = header fakeHeaders
         ,cycles = (1 + length drivingz)
        }

type Pin = Word8

digitalWrite :: Pin -> V Word8 -> Atom ()
digitalWrite p v = action (\[x, y] -> printf "digitalWrite(%s, %s)" x y) [p', v']
    where p' = ue $ Const p
          v' = ue $ value v

drive :: V Word8 -> V Word8 -> Atom()
drive s t = action (\[x, y] -> printf "STEERING(%s); THROTTLE(%s)" x y) [s', t']
  where s' = ue $ value s
        t' = ue $ value t

assignArray :: Assign a => [(V a, E a)] -> Atom()
assignArray = mapM_ $ uncurry (<==)

-- Main just has to compile the Atom expression
main :: IO ()
main = do 
        (schedule, _, _, _, _) <- compile "main" config navigate
        runTests 0 [do return t1, do return t2]
        -- putStrLn $ reportSchedule schedule
        return ()

variables :: Expr a => A (a) -> [V a]
variables a = map ((a !) . Const) indices

values :: Expr a => A (a) -> [E a]
values = map value . variables

indices :: [Int16]
indices = take 5 [0..]

random' :: String -> Atom (E Int16)
random' x = int16' ("rand(); // yes i know " ++ x) >>= (return . value)

compass :: Atom (E Int16)
compass = int16' "COMPASS" >>= (return . value)

drivingz = regularDrivings ++ [(0, 0, 90)] ++ closeToBottom ++ [(0, 0, 105)] ++ closeToTop ++ [(0, 0, 75)] ++ wrongDirection ++ [(0, 0, 0)]

regularDrivings = [
       (1000, 800,  105)
     , (1000, 900,  105)
     , (1000, 1001, 90)
     , (1000, 900,  90)
     , (1000, 1001, 90)
     , (1000, 1101, 75)
     , (1000, 1001, 75)     
     , (1000, 999,  90)
     ]

closeToBottom = [
       (50, 3500, 105)
     , (50, 51, 90)  
     , (50, 151, 75)
     , (50, 51, 75)
     , (50, 49, 90)
     , (50, 3549, 105)
     , (50, 49, 105)    
     ]

closeToTop = [
       (3550, 3400, 105)
     , (3550, 3549, 105) 
     , (3550, 3551, 90)
     , (3550, 51,   75)
     , (3550, 3590, 75)
     , (3550, 500,  75)
     ]

wrongDirection = [
       (1500, 3500, 105)
     , (1500, 2500, 75)
     , (1500, 3350, 105)
     , (1500, 3301, 105)
     , (1500, 3250, 75)
     , (1500, 3300, 75)
     ]

steerings = [steering | (_, _, steering) <- drivingz]

steeringVsBearing = do
     steering <- word8' "steering"
     compass <- int16' "compass"
     targetHeading <- int16' "targetHeading"
     actualHeading <- int16' "actualHeading"
     targets <- array "targets" ([target | (target,_, _) <- drivingz])
     actuals <- array "actuals" ([actual | (_, actual, _) <- drivingz])
     steerings <- array "steerings" (head steerings : steerings) 
     targetHeading <== targets !. clock
     compass <== actuals !. clock
     navigate
     -- fix this :|
     assert "steering works" $
           (value steering) ==. (steerings !. clock) ||.
           (value steering) ==. (steerings !. (clock - 1))

     printIntegralE "clock" clock

arraySumming :: Atom ()
arraySumming = do
     lolDongs <- array "lolDongs" [1,2,3,4]
     arraySum <- int16 "arraySum" 10
     regularSum <- int16 "regularSum" 10
     regularSum' <- int16 "regularSum'" 10

     randoms <- do
                  r <- mapM (random' . (\x -> [x])) $ take 5 ['a'..] 
                  return r
                  
     lolDongs <=== randoms

     arraySum <== foldl1 (+) (values lolDongs)
     regularSum <== foldl1 (+) randoms
     regularSum' <== value regularSum

     printIntegralE "arraySum" $ value arraySum
     printIntegralE "regularSum'" $ value regularSum'
     
     assert "sum is calculated" ((value arraySum) ==. (value regularSum'))

infixr 1 <===
(<===) :: Assign a => A a -> [E a] -> Atom()
a <=== xs = assignArray $ (zip (variables a) xs)

distanceThreshold = 1200

headingThreshold = Const 100

isLarge :: V Int16 -> E Bool
isLarge v = value v >. distanceThreshold

dodgeAmount :: E Int16 -> E Int16 -> E Word8
dodgeAmount l r = Const 10

navigate :: Atom ()
navigate = do
    high <- word8' "HIGH"
    
    low <- word8' "LOW"

    dodging <- bool "dodging" False

    moving <- bool "moving" False

    dodgeSteering <- int16 "dodgeSteering" 0

    leftDistanceMeasurements <- array "leftDistanceMeasurements" (map (\_ -> (0::Int16)) indices)
    rightDistanceMeasurements <- array "rightDistanceMeasurements" (map (\_ -> (0::Int16)) indices)

    newestRight <- int16' "newestRight" 
    newestLeft <- int16' "newestLeft"

    leftSum <- int16 "leftSum" 0
    rightSum <- int16 "rightSum" 0

    heading <- compass

    targetHeading <- int16' "targetHeading"
    actualHeading <- int16' "actualHeading"

    steering <- word8' "steering"
    throttle <- word8 "throtle" 0

    period 1 $ atom "read compass" $ do
        actualHeading <== heading

    period 1 $ atom "steering control left" $ do
        actual'' <- do return $ value actualHeading
        target'' <- do return $ value targetHeading
        actual' <- do return $ mux (target'' - actual'' >. 1800) (actual''+3600) (actual'')
        target' <- do return $ mux (actual'' - target'' >. 1800) (target''+3600) (target'')
        printIntegralE "target'" target'
        printIntegralE "actual'" actual'
        printIntegralE "difference" (target'-actual')

        assert "target is closer than 1800 to actual" (target' - actual' <=. 1800)
        assert "target is closer than 1800 to actual'" (actual' - target' <=. 1800)
        l1 <- atom "left" $ do
          tooMuchLeft' <- hysteresis (target' - headingThreshold) target' actual'
          return tooMuchLeft'
        r1 <- atom "right" $ do
          tooMuchRight' <- hysteresis target' (target' + headingThreshold) actual'
          return tooMuchRight'
        steering' <- do return $ mux l1 (mux r1 75 90) 105
        steering <== steering'
        printIntegralE "steering" (steering')
        printStrLn "-------------------------------------------"

    period 5 $ atom "drive" $ do
        steering' <- do return $ mux (value dodging) 0 (value steering) 
        drive steering throttle

    period 2 $ atom "calculate dodge" $ do
        cond $ value dodging 
        cond $ or_ [isLarge leftSum, isLarge rightSum]
        dodgeSteering <== value leftSum - value rightSum

    period 1 $ atom "distance measurements" $ do
        cond $ value dodging 
        lefts <- do return $ values' newestLeft leftDistanceMeasurements
        rights <- do return $ values' newestRight rightDistanceMeasurements
        leftDistanceMeasurements <=== lefts
        rightDistanceMeasurements <=== rights

        leftSum <== foldl1 (+) lefts
        rightSum <== foldl1 (+) rights

          where
          values' :: Expr a => V a -> A a -> [E a]
          values' x a = value x : values a
